/*
 * @Author: ssyyw 2294779310@qq.com
 * @Date: 2024-03-29 00:07:18
 * @LastEditors: ssyyw 2294779310@qq.com
 * @LastEditTime: 2024-03-31 16:13:08
 * @FilePath: \Work\mavproxy\ws_ros1\src\mavproxy_ros1\src\drone_mode.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */

#ifndef DRONE_DEFS
#define DRONE_DEFS

#include <string>
#include <queue>
#include "mavproxy_ros1/FlightCmd.h"
#include "mavproxy_ros1/FlightState.h"
#include "mavproxy_ros1/FlightControll.h"

const int RETSUCCESS = 0;
const int RETFAILED = 1;

// -----------------------------飞行模式 (APM) -----------------------------
const std::string ACRO_APM_MODE = "ACRO";
const std::string KEEP_APM_MODE = "LOITER";
const std::string LAND_APM_MODE = "LAND";
const std::string TAKEOFF_APM_MODE = "STABILIZE";
const std::string GUIDED_APM_MODE = "GUIDED";
const std::string POSITION_APM_MODE = "POSCTL";
const std::string RETURN_APM_MODE = "RTL";
const std::string AUTO_APM_MODE = "AUTO";

// -----------------------------飞行模式 (PX4) -----------------------------
const std::string STABLIZE_PX4_MODE = "STABLIZE";
const std::string TAKEOFF_PX4_MODE = "AUTO.TAKEOFF";
const std::string LAND_PX4_MODE = "AUTO.LAND";
const std::string HOLD_PX4_MODE = "AUTO.LOITER";
const std::string RETURN_PX4_MODE = "RTL";
const std::string OFFBOARD_PX4_MODE = "OFFBOARD";
const std::string MISSION_PX4_MODE = "MISSION";
//  Hold, Return, Mission, Takeoff, Land, Follow Me, Offboard

// -----------------------------发布主题----------------------------
const std::string MAVROS_SET_LOCAL_POSE = "/mavros/setpoint_position/local";
const std::string MAVROS_SET_GLOABL_POSE = "/mavros/setpoint_position/global";
const std::string MAVROS_SET_VELOCITY = "/mavros/setpoint_velocity/cmd_vel_unstamped";
const std::string MAVROS_SET_ACCEL = "/mavros/setpoint_accel/accel";
const std::string MAVROS_SET_ATTITUDE = "mavros/setpoint_attitude/attitude";
const std::string MAVROS_SET_LOCAL_POSITION = "/mavros/setpoint_position/local";
const std::string LOCAL_PLANNER_GOAL_POSITION = "/mavproxy_ros1/flight_goal";

// -----------------------------服务客户端----------------------------
const std::string MAVROS_SRV_ARM = "/mavros/cmd/arming";
const std::string MAVROS_SRV_MODE = "/mavros/set_mode";
const std::string MAVROS_SRV_TAKEOFF = "/mavros/cmd/takeoff";
const std::string MAVROS_SRV_LAND = "/mavros/cmd/land";
const std::string MAVROS_SET_ROTATE = "/mavros/cmd/command";
const std::string MAVROS_SET_HOME = "/mavros/cmd/set_home";
const std::string MAVPROXY_CONTROLL = "/mavproxy_ros1/controll";

// -----------------------------订阅主题----------------------------
const std::string MAVROS_INFO_STATE = "/mavros/state";
const std::string MAVROS_INFO_TIME = "/mavros/time_reference";
const std::string MAVROS_INFO_LOCAL_POSE = "/mavros/local_position/pose";
const std::string MAVROS_INFO_VELOCITY = "/mavros/local_position/velocity";
const std::string MAVROS_INFO_GPS_POS = "/mavros/global_position/global";
const std::string MAVROS_SET_RETURN = "/mavros/home_position/home";

// ------------------------------自定义消息----------------------------
const std::string MAVPROXY_INPUT_CMD = "/mavproxy/flight_input_cmd";
const std::string MAVPROXY_PARSED_CMD = "/mavproxy/flight_parsed_cmd";
const std::string MAVPROXY_FLIGHT_STATE = "/mavproxy/flight_state";

// -------------------------------飞行命令---------------------------
const std::string CMD_FLIGHT_TAKEOFF = "flight_takeoff";
const std::string CMD_FLIGHT_LAND = "flight_land";
const std::string CMD_FLIGHT_HOVER = "flight_hover";
const std::string CMD_FLIGHT_FLY_POSE = "flight_targetpos";
const std::string CMD_FLIGHT_FLY_GPOSE = "flight_targetgpos";
const std::string CMD_FLIGHT_ROTATEH = "flight_rotateh";
const std::string CMD_FLIGHT_RETURN = "flight_return";
const std::string CMD_FLIGHT_SETHOME = "flight_sethome";
const std::string CMD_FLIGHT_SETHSPEED = "flight_setspeed";
const std::string CMD_FLIGHT_STOP = "flight_stop";
const std::string CMD_FLIGHT_ARM = "flight_arm";
const std::string CMD_FLIGHT_DISARM = "flight_disarm";
// -------------------------------飞控版本 ---------------------------
const std::string FMU_PX4 = "PX4";
const std::string FMU_APM = "APM";

const int GLOBAL_TIMEOUT_LIMIT = 10;
const int GLOBAL_RETRY_TIMES = 3;
const int GLOBAL_DEFAULT_QUEUE_SIZE = 20;
const int GLOBAL_CONTROLL_QUEUE_SIZE = 10;

const int GLOBAL_LOCAL_X_PARAM_LIMIT = 10;
const int GLOBAL_LOCAL_Y_PARAM_LIMIT = 10;
const int GLOBAL_LOCAL_Z_PARAM_LIMIT = 2;
const float GLOBAL_ENABLE_MESSAGE_DURATION = 5.0;
const float DRONE_CONNECTION_TIMEOUT_DURATION = 10.0;
const float GLOBAL_COMMON_ROS_RATE = 20.0;
const float GLOBAL_REACHED_DISTANCE = 1;
const float GLOBAL_SLEEP_WAIT_TIME = 1.0;
const float DEFAULT_TAKEOFF_HEIGHT = 1.5;

const float PI = 3.14;

const double MOTORS_SPEEDUP_TIME = 3.0; // motors idle running for 3 seconds before takeoff
const double DELAY_TRIGGER_TIME = 2.0;  // Time to be delayed when reach at target height

// 定义本地坐标系下结构体，方便用工具类计算坐标距离
struct LocalPoint
{
    float x;
    float y;
    float z;
    template<typename T1, typename T2, typename T3>
    LocalPoint(T1 x, T2 y, T3 z)
    {
        this->x = static_cast<float>(x);
        this->y = static_cast<float>(y);
        this->z = static_cast<float>(z);
    }
};

// 定义飞行任务状态
enum TaskState
{
    START,
    HANDLE,
    FINISH,
    BREAK,
    FAIL,
};

// 定义任务信息，所有任务都只有命令和参数列表，handler下发后到fsm，fsm解析后直接调用controller中的控制函数
// 当前不存在任务队列的说法，需要排队任务在更上一层实现
struct TaskInfo
{
    int taskId;
    TaskState taskState;
    mavproxy_ros1::FlightCmd taskCmd;
    
    TaskInfo(){
        taskId = 0;
        taskState = TaskState::START;
    }
};

// 定义状态机状态
enum DroneState
{
    STABLIZE,
    TAKEOFF,
    LAND,
    HOLD,
    RETURN,
    MISSION,
    OFFBOARD
};

// 定义当前控制器的整体状态
struct MavproxyState
{
    bool isConnected;          // 当前是否已经连接上无人机
    bool isRcBreakIn;          // 当前是否有遥控器切入控制
    bool isSafeBreakIn;        // 当前是否有安全保护触发接管
    DroneState droneStatus;   // 当前无人机状态
};


#endif